#include "module_base/publish_test_nodelet.h"

namespace module_base
{

void PubTestNodelet::onInit()
{
	init_nodelet();
	// subscribers_["input"]=nh_.subscribe(params_["input_topic"],
	// 	10,&TestNodelet::test_callback,this);
	// publishers_["output"]=nh_.advertise<std_msgs::String>(
	// 	params_["output_topic"],10);
	CREATE_PUBLISHER<sensor_msgs::Image>("image_test","/image_test",10);
	thread_=std::thread(std::bind(&PubTestNodelet::pub_test,this));
	CREATE_SUBSCRIPION(params_["input_topic"],
		&PubTestNodelet::test_callback,10);


}

void PubTestNodelet::pub_test()
{
	while (ros::ok())
	{
		std::srand((unsigned)time(NULL));
		sensor_msgs::Image test_image;
		int width=1920;
		int height=1080;
		test_image.data.resize(width*height*3);
		for (int i=0;i<width*height*3;i++)
		{
			test_image.data[i]=rand()%255;
		}
		test_image.header.stamp=ros::Time::now();
		test_image.height=height;
		test_image.width=width;
		test_image.encoding="rgb8";
		test_image.is_bigendian=false;
		test_image.step=width*3;
		publishers_["image_test"].publish(test_image);
		printf ("pub image\n");
		usleep(1000);
	}
}

void PubTestNodelet::test_callback(const std_msgs::StringConstPtr& msg)
{
	ROS_INFO("I heared: %s",msg->data.c_str());
	std_msgs::String output_msg;
	output_msg.data=msg->data;
	publishers_["output"].publish(output_msg);
}

}

PLUGINLIB_EXPORT_CLASS(module_base::PubTestNodelet, nodelet::Nodelet)